#include <iostream>
#include <fstream>
#include <ctime>
#include <cstdlib> 
#include <stdio.h>
#include <time.h>

#include <fstream>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp" //inclui

using namespace cv;
using std::ifstream;
using std::ofstream;

//inclui 
using namespace std;

#include "epnp.h"

const double uc = 320;
const double vc = 240;
const double fu = 800;
const double fv = 800;


// MtM takes more time than 12x12 opencv SVD with about 180 points and more:

const int n = 10;
double noise;

double rand(double min, double max)
{
  return min + (max - min) * double(rand()) / RAND_MAX;
}

void random_pose(double R[3][3], double t[3])
{
  const double range = 1;

  double phi   = rand(0, range * 3.14159 * 2);
  double theta = rand(0, range * 3.14159);
  double psi   = rand(0, range * 3.14159 * 2);

  R[0][0] = cos(psi) * cos(phi) - cos(theta) * sin(phi) * sin(psi);
  R[0][1] = cos(psi) * sin(phi) + cos(theta) * cos(phi) * sin(psi);
  R[0][2] = sin(psi) * sin(theta);

  R[1][0] = -sin(psi) * cos(phi) - cos(theta) * sin(phi) * cos(psi);
  R[1][1] = -sin(psi) * sin(phi) + cos(theta) * cos(phi) * cos(psi);
  R[1][2] = cos(psi) * sin(theta);

  R[2][0] = sin(theta) * sin(phi);
  R[2][1] = -sin(theta) * cos(phi);
  R[2][2] = cos(theta);

  t[0] = 0.0f;
  t[1] = 0.0f;
  t[2] = 6.0f;
}

void random_point(double & Xw, double & Yw, double & Zw)
{
  double theta = rand(0, 3.14159), phi = rand(0, 2 * 3.14159), R = rand(0, +2);

  Xw =  sin(theta) * sin(phi) * R;
  Yw = -sin(theta) * cos(phi) * R;
  Zw =  cos(theta) * R;
}

void project_with_noise(double R[3][3], double t[3],
			double Xw, double Yw, double Zw,
			double & u, double & v)
{
  double Xc = R[0][0] * Xw + R[0][1] * Yw + R[0][2] * Zw + t[0];
  double Yc = R[1][0] * Xw + R[1][1] * Yw + R[1][2] * Zw + t[1];
  double Zc = R[2][0] * Xw + R[2][1] * Yw + R[2][2] * Zw + t[2];

  double nu = rand(-noise, +noise);
  double nv = rand(-noise, +noise);
  u = uc + fu * Xc / Zc + nu;
  v = vc + fv * Yc / Zc + nv;
}

int main(int /*argc*/, char ** /*argv*/)
{
    ofstream testeopencv;
  testeopencv.open ("/home/alex/Documentos/testeopencv.txt");

testeopencv <<"noise"<<",";
testeopencv << "err2EPNP"<<",";
testeopencv << "rot_err"<<",";
testeopencv << "transl_err"<<",";
testeopencv << "solvepnperr"<<",";
testeopencv << "rot_errOCV"<<",";
testeopencv << "transl_errOCV"<<",";
testeopencv<<"\n";
  
//varíaveis cvmat
  Mat rvec; 
  Mat tvec; 
  Mat matrizrvec;
  Mat jacobian;
 
   Mat objectpointmat(n,3,CV_64FC1);//matriz dos objetos

  Mat imagepointsmat(n,2,CV_64FC1); //matriz dos pontos a imagem
  // declaração de matriz rvec C++;
  int cols = matrizrvec.cols;
  int linha = matrizrvec.rows;
  
  double matrixrvec [3][3];
  
  //declaração de matriz tvec c ++
  int j = tvec.cols;
  double vetortvec [3];  
  
  epnp PnP;

  srand(time(0));
// varia noise
 for (noise = 0.0; noise < 10.0; noise = noise + 0.5) {
 
   //muda os pontos aleatórios.
 for (int contadr = 0; contadr < 100; contadr = contadr + 1) {
  PnP.set_internal_parameters(uc, vc, fu, fv);
  PnP.set_maximum_number_of_correspondences(n);

  double R_true[3][3], t_true[3];
  random_pose(R_true, t_true);

  PnP.reset_correspondences();
 
 
  
  for(int i = 0; i < n; i++) {
    double Xw, Yw, Zw, u, v;

    random_point(Xw, Yw, Zw);

    project_with_noise(R_true, t_true, Xw, Yw, Zw, u, v);
    PnP.add_correspondence(Xw, Yw, Zw, u, v);
  
     //adicionar pontos a matriz objectpointmat
    objectpointmat.at<double>(i,0)=Xw;
    objectpointmat.at<double>(i,1)=Yw;
    objectpointmat.at<double>(i,2)=Zw;

    //adicionar pontos a matriz imagepointsmat
    imagepointsmat.at<double>(i,0)=u;
    imagepointsmat.at<double>(i,1)=v;
    }
  

  double m[3][3] = {{fu,0,uc}, {0,fv,vc}, {0,0,1}};
 Mat camera = Mat(3, 3, CV_64FC1, m);
 double d[4] = {0,0,0,0};
 Mat dist = Mat(1, 4, CV_64FC1, d);

  
  cv::solvePnP (objectpointmat,imagepointsmat,camera,dist,rvec,tvec, false);   
  
  double R_est[3][3], t_est[3];
  double err2 = PnP.compute_pose(R_est, t_est);
  double rot_err, transl_err;

  PnP.relative_error(rot_err, transl_err, R_true, t_true, R_est, t_est);
  cout << ">>> Reprojection error: " << err2 << endl;
  cout << ">>> rot_err: " << rot_err << ", transl_err: " << transl_err << endl;
  cout << endl;
  
  double errtrue = PnP.reprojection_error(R_true, t_true); 
  cout << "'True reprojection error':"
  << errtrue << endl;
  cout << endl;
  
  cout << "True pose:" << endl;
  PnP.print_pose(R_true, t_true);
  cout << endl;
  cout << "Found pose:" << endl;
  PnP.print_pose(R_est, t_est);

  cout << "\n*****OCV*****"; 
  cout << "\ncamera"; 
  cout << camera;
  cout << "\nobjpts"; 
  cout << objectpointmat;
  cout << "\nimgpts"; 
  cout << imagepointsmat;
  cout << "\n rvec" << endl;
  cout << rvec;
  cout << "\n tvec" << endl;
  cout << tvec;
  
  //  transforma o rvec em uma matriz.
  
  cv::Rodrigues(rvec,matrizrvec,jacobian);
  
  cout <<"\n matrizrvec"<<endl;
  cout << matrizrvec;
  
  //coloca elementos na matriz c++ 
  cout <<"\n matrixrvec c++" <<endl;
  
  for (int linha = 0; linha < 3; linha++) {
      
    for (int cols = 0; cols < 3; cols++) {
      
      matrixrvec[linha][cols] = matrizrvec.at<double> (linha,cols);
 
       
      cout<< matrixrvec[linha][cols];
    
     cout<<"\n"<< endl;
    }
  
  }
  
  //colocar elementos na matriz tvec c++
  cout <<"\n matrixtvec c++" <<endl;
   for (int j = 0; j < 3; j++) {
      
      vetortvec[j] = tvec.at<double> (0,j);

        cout<<vetortvec[j];
    
     cout<<"\n"<< endl;      
    }
  
 double solvepnperr = PnP.reprojection_error(matrixrvec, vetortvec);
  
  cout << "SolvePNP reprojection error"
       << solvepnperr << endl;
  
  cout << "\n";
 
//erro relativo do solvePnP

  double rot_errOCV;
  double transl_errOCV;
  PnP.relative_error(rot_errOCV, transl_errOCV, R_true,t_true, matrixrvec, vetortvec);

  cout << ">>> Reprojection error OCV: " << err2 << endl;
  cout << ">>> rot_errOCV: " << rot_errOCV << ", transl_errOCV: " << transl_errOCV << endl;
  cout << endl;
  // guarda os dados.
  

 testeopencv <<noise<<",";
  testeopencv << err2<<",";
testeopencv << rot_err<<",";
testeopencv << transl_err<<",";
testeopencv << solvepnperr<<",";
testeopencv << rot_errOCV<<",";
testeopencv << transl_errOCV<<",";
testeopencv<<"\n";
} //fim da variação da const
     }//fim da variaçãpo dop noise

testeopencv.close();
  
  
  return 0;
}
